#include "State_Jump2.h"
#include "Dog.h"

#define POINTANGLE RAD(100)
#define KP1 4.0f
#define KD1 0.6f

enum jump_state
{
    READY_STATE,
    JUMPING_STATE,
    SWING_STATE,
    FALLING_STATE,
} jumpState;

void Jump2_Enter(void)
{
    UART_Print("Jump2 enter\n");

    /* 蹬腿跳跃 */
    HT_SetPosition(dog.legs[0].motorF, ANGLE_F + global.jumpAngle + POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F - global.jumpAngle - POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F - global.jumpAngle - POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F + global.jumpAngle + POINTANGLE, KP1, KD1, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B - global.jumpAngle + POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B + global.jumpAngle - POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B + global.jumpAngle - POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B - global.jumpAngle + POINTANGLE, KP1, KD1, 0);

    jumpState = JUMPING_STATE; // 跳跃状态中
}

#define KP2 4.0f
#define KD2 0.3f
#define KP3 1.0f
#define KD3 1.3f

void Jump2_Run(void)
{
    // UART_Print("Jump2 run\n");

    switch (jumpState)
    {
    case JUMPING_STATE:
        if (HT_ArrivalPos(dog.legs[0].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[1].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[2].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[3].motorF, 0.1))
        {
            /* 空中摆腿 */
            HT_SetPosition(dog.legs[0].motorF, ANGLE_F - global.jumpAngle, KP2, KD2, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F + global.jumpAngle, KP2, KD2, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F + global.jumpAngle, KP2, KD2, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F - global.jumpAngle, KP2, KD2, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B + global.jumpAngle, KP2, KD2, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B - global.jumpAngle, KP2, KD2, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B - global.jumpAngle, KP2, KD2, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B + global.jumpAngle, KP2, KD2, 0);

            jumpState = SWING_STATE; // 摆动状态中
        }
        break;

    case SWING_STATE:
        if (HT_ArrivalPos(dog.legs[0].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[1].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[2].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[3].motorF, 0.1))
        {
            /* 落地阻尼 */
            HT_SetPosition(dog.legs[0].motorF, ANGLE_F - global.jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F + global.jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F + global.jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F - global.jumpAngle, KP3, KD3, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B + global.jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B - global.jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B - global.jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B + global.jumpAngle, KP3, KD3, 0);

            jumpState = FALLING_STATE; // 落地状态中
        }
        break;

    default:
        break;
    }
}

void Jump2_Exit(void)
{
    UART_Print("Jump2 exit\n");
}